#include <cmath>
#include <ctime>
#include <iostream>
#include <cstdio>
#include "Setting.h"
#include "FEMModel.h"
#include "DeformationState.h"
#include "Tetrahedron.h"
#include "CutPerformer.h"
#include "Statistics.h"
#include "TopologicalOperation.h"
#include "Collapse.h"

// other files

using namespace std;



int warncount;

void DefaultBatchSimulate (FiniteElementModel* fem)
{
	int max_mflops_ =   (int) GetNumberSetting ("maximum-mflops");

	while (1)
	{
		DeformationState *ds =      fem->pstate_;

		ds->SimulationBody ();
		ds->ValidateNumericalSanity();

		/* StaticDeformationState* sp = dynamic_cast<StaticDeformationState*> (ds);
		
		if (sp&& sp->GoodSolution ())
			break;

		if ( (flop_count * 1e-6)  > max_mflops_)
		{
			printf( "FLOP maximum reached.\n");
			break;
		} */

		ds->FlopStats ();
	}

	Real mean_dv = 0.0;
	Real max_dv = 0.0;  
	LinkArray<Tetrahedron> tets = fem->TetrahedronArray ();
	for (int i = tets.Size(); i--;)
	{
		Real vol = tets[i]->Volume(true);
		Real fixvol = tets[i]->Volume(false);
		Real dv = fabs(vol - fixvol)/ fixvol;
		max_dv = max_dv >dv?max_dv: dv ;
		mean_dv = mean_dv + dv;
	}

	mean_dv /= tets.Size ();
	printf ("Volume change: average %d %%, max %d %%\n", int (rint (mean_dv*100)), int (rint (max_dv*100)));   

	exit (0);

} 





#if 0
/*
Check formulas: for a single tetrahedron calculate

nonLinearElasticForce (U+H) -nonLinearElasticForce(U)

compare with

nonLinearElasticForceDerivative (U) * H

also compare

nonLinearElasticForceDerivative (U+H) and
linearElasticForce(U+H)

which should be O(|H|^2)

*/
void CheckTetFormulas(FiniteElementModel*fem)
{
	TetrahedronState * tet = fem->TetrahedronArray ()[0]->state_;

	Array<Real> ha, lfa, fa, fpha, da, dera, f2a, hsa,scra;

	Real* h, *lf,* f, *fph,* d,* der,* f2, *hs,*scr ;
	da = fem->pstate_->displacement_;

	int n =da.Size ();

	ha.SetSize (n);
	fa.SetSize (n);
	f2a.SetSize (n);  
	fpha.SetSize (n);
	dera.SetSize (n);
	lfa.SetSize (n);
	hsa.SetSize (n);
	scra.SetSize (n);

	h = ha.UnsafeAccessArray();
	lf = lfa.UnsafeAccessArray ();
	f = fa.UnsafeAccessArray ();
	fph = fpha.UnsafeAccessArray ();
	d = da.UnsafeAccessArray ();
	der = dera.UnsafeAccessArray ();
	f2 = f2a.UnsafeAccessArray ();
	hs = hsa.UnsafeAccessArray ();
	scr = scra.UnsafeAccessArray ();

	BigVectorNullify (lf, n);
	BigVectorNullify (h, n);
	BigVectorNullify (f, n);
	BigVectorNullify (f2, n);
	BigVectorNullify (scr,n);  
	BigVectorNullify (fph, n);
	BigVectorNullify (der, n);

	d[4]   += 0.1;
	NonlinearElasticForce (tet, f, d);
	NonlinearForceDerivative (tet, der, f2, d, h);

#if 0
	cout << "force only - force from derivative = " << BigVectorDistance (f2, f, n) <<endl;
	cout << "displ: "<<endl;
	BigVectorPrint (d, n);
	cout << "deriv+force: "<<endl;
	BigVectorPrint (f2, n);
	cout << "force: "<<  endl;
	BigVectorPrint (f, n);

	BigVectorAdd (d, d, h, n);  
#endif

	NonlinearElasticForce (tet, fph, d);
	LinearElasticForce (tet, lf, d);
	printf(  "Linear - nonlinear force: %f\n", big_vector_distance (fph, lf, n));
}


void TestForceDerivative (FiniteElementModel *fem)
{
	TetrahedronState * tet = fem->TetrahedronArray ()[0]->state_;
	CheckTetFormulas (fem);


	Array<Real> ha, lfa, fa, fpha, da, dera, f2a, hsa,scra;

	Real* h, *lf,* f, *fph,* d,* der,* f2, *hs,*scr ;
	da = fem->pstate_->displacement_;

	int n =da.Size ();

	ha.SetSize (n);
	fa.SetSize (n);
	f2a.SetSize (n);  
	fpha.SetSize (n);
	dera.SetSize (n);
	lfa.SetSize (n);
	hsa.SetSize (n);
	scra.SetSize (n);

	h = ha.UnsafeAccessArray();
	lf = lfa.UnsafeAccessArray ();
	f = fa.UnsafeAccessArray ();
	fph = fpha.UnsafeAccessArray ();
	d = da.UnsafeAccessArray ();
	der = dera.UnsafeAccessArray ();
	f2 = f2a.UnsafeAccessArray ();
	hs = hsa.UnsafeAccessArray ();
	scr = scra.UnsafeAccessArray ();

	BigVectorNullify (lf, n);
	BigVectorNullify (h, n);
	BigVectorNullify (f, n);
	BigVectorNullify (f2, n);
	BigVectorNullify (scr,n);  
	BigVectorNullify (fph, n);
	BigVectorNullify (der, n);

	d[4]   += 0.1;

	h[tet->incident_nodes_[0]] =1.0;

	Real scale = 1e-6;
	BigVectorScale (hs, scale, h, n);
	printf ("scale = %f\n", scale);

	fem->pstate_->ApplyForceDerivativeAndResidual (der,
		f,
		d,
		h);
	fem->pstate_->ComputeResidualForce (f2, d);
	printf ("residual() - derivative->residual ()  = %f\n",  BigVectorDistance (f2, f, n)); 

	Real norm_deriv =  BigVectorIP (f, der, n) / sqrt (BigVectorIP (f,f, n));

	printf ("d/dt [t -> ||res(U + tH)||] = %f \n", norm_deriv);

	BigVectorNullify (der,n);
	BigVectorNullify (f, n);
	BigVectorPrint (d, n);
	fem->pstate_->ApplyForceDerivativeAndResidual (der,
		f,
		d,
		hs);
	BigVectorAdd (d, d, hs, n);
	BigVectorPrint (d, n);  
	fem->pstate_->ComputeResidualForce (fph, d);

	printf ("d/dt [t -> ||res(U + tH)||] (estd.) %f\n",
		(sqrt (BigVectorIP (fph, fph, n)) - sqrt (BigVectorIP (f,f, n))) / scale);


	cout << "ext. force" <<endl;
	BigVectorPrint (fem->pstate_->external_force_.AccessArray (),n);

	cout << "F(U+H)" << endl ;
	BigVectorPrint (fph, n);
	cout << "F(U)" << endl;
	BigVectorPrint (f, n);
	BigVectorSubtract (fph, fph, f, n);
	cout << "F(U+H) -F (U)" <<endl;
	BigVectorPrint (fph,n);
	cout << "DF[H]" <<endl;
	BigVectorPrint (der,n);
	BigVectorSubtract (fph, fph, der,n);
	cout << "difference" <<endl;
	BigVectorPrint (fph,n);

}
#endif


/*
perform relaxation and cuts in batch. 
*/
void SimulateSingleCut (FiniteElementModel* fem)
{
	int count = int (GetUpdateFactor () * 3000.0 / fem->NodeCount ());
	if (count < 1) 
		count = 1;
	if (!relaxation_global_b)
		count = 0;

	CutPerformer *cp = new CutPerformer (fem);
	SweepDescription this_sweep;
	this_sweep.SetInterestingDefault (fem);
	Vector3 vel = this_sweep.points_[2] - this_sweep.points_[1];

	int steps = 5; // (int) rint ( 3.0 / vel.length ());

	for (int i=0; i < steps; i++)
	{
		for (int k =   count; k--;)
			fem->pstate_->SimulationBody ();


		cp->SetNextSweep (this_sweep);

		cp->DoCut ();

		this_sweep = this_sweep.NextSweep (vel);
	}

	cout << "Batch cutting finished.  " <<endl;
	if (!visualize_global_b)
		exit (0);
} 



/*
Write file with aspect-ratio of every tet. 
*/
void WriteFatnessGraph (FiniteElementModel*fem)
{
	char s[1024];
	sprintf (s, "stats/aspectratio-%d", CutPerformer::total_cut_count_); 
	fprintf (stderr, "Writing fatness graph to %s... " , s);
	fflush (stderr);

	FILE * f = fopen (s, "w");
	if (!f)
	{
		fprintf (stderr, "can't open file\n");
		return ; 
	}

	fputc ('#', f);
	fputs (FEMStatistics(fem), f);
	fputc ('#', f);
	fputs (CutPerformerStatistics (), f);
	fputc ('#', f);
	fputs (EdgeCollapseStatistics (), f);
	fputc ('#', f);
	fputs (CollapseStatistics (), f);

	fputs ("# format: aspect, min dih angle, PI - max dih angle\n", f);

	LinkArray<Tetrahedron> tets (fem->TetrahedronArray ());
	for (int i = tets.Size (); i--;)
	{
		Tetrahedron *t = tets[i];
		Real fat = AspectRatio (t, false);
		Real min, max;
		MinmaxDihedralAngle (t, &min, &max, false);

		max = M_PI - max;
		fprintf(f, "%f %f %f\n", fat, min, max);
	}
	fclose (f);
	fprintf (stderr,"done\n" );
}



/*
do a few cuts, then do a single relaxation run.

* Objective: see how cuts and node snapping influences convergence.

* Stress test degeneracy handling routines.

*/

// rotate the scalpel. Symmetric results for a  
void SliceNDice (FiniteElementModel *fem)
{
	const int slices = 5;
	for (int i = 0 ; i< slices ; i++)
	{
		//      set_setting  ("scalpel-x-start", 1.5 *(0.5 - i / Real (slices)));

		SetNumberSetting  ("scalpel-angle", 0.25 + 1.0 *(0.5 - i / Real (slices)));      

		CutPerformer *cp = new CutPerformer (fem);
		SweepDescription this_sweep;
		this_sweep.SetInterestingDefault (fem);
		Vector3 vel = this_sweep.points_[2] - this_sweep.points_[1];

		int steps = (int) rint ( 3.0 / vel.Length ())  + 1;
		steps = 2;

		for (int i=0; i < steps; i++)
		{
			fem->pstate_->SimulationBody ();
			cp->SetNextSweep (this_sweep);
			cp->DoCut ();
			this_sweep = this_sweep.NextSweep (vel);
		}
	}

	cout << "Batch cutting finished.  " <<endl;



	if (!visualize_global_b)
		exit (0);  
}


/*
Apply a collapse with two nodes.  (27,39) happens to be on the frnot
for a 4x4x4 generated cube.  */

void TestCollapse (FiniteElementModel* fem)
{
#if 0
	for (int i=0; i < 2;i++)
	{
		Node *n[] = {fem->GetNode (0), fem->GetNode(1)}; 
		Simplex e (EDGE,n, true);

		cout << "Collapsing ... "<<flush;
		TryCollapseEdge (fem, e);
	}

	fem->Validate ();
	fem->state_->ScheduleReinit ();


	cout << "done " <<endl;
#endif
}

void TestSplit (FiniteElementModel*fem)
{
#if 0
	for (int i=0; i < 1;i++)
	{
		Node *n[] = {fem->GetNode (0), fem->GetNode(1)}; 
		Simplex e (EDGE,n, true);

		cout << "Splitting  ... "<<flush;
		TrySplitEdge (fem, e);
	}

	fem->Validate ();
	fem->state_->ScheduleReinit ();



	cout << "done " <<endl;
#endif
}


void TestSplitTri (FiniteElementModel*fem)
{
#if 0
	fem->ValidateObjects ();
#endif
#if 0
	Tetrahedron *tet = (Tetrahedron*)fem->FindTopolObject (2930,TETRAHEDRON);
	Node * ns[] = { fem->GetNode (849), fem->GetNode (858) };

	Simplex ts(EDGE, ns, 0);


	TryCollapseEdge (fem, ts, tet , 0);
#endif
#if 0
	Tetrahedron *tet = (Tetrahedron*)fem->FindTopolObject (5,TETRAHEDRON);
	Node * ns[] = { fem->GetNode (7), fem->GetNode (9) };

	Simplex ts(EDGE, ns, 0);


	TryCollapseEdge (fem, ts, tet , 0);
#endif
	Tetrahedron *tet = (Tetrahedron*)fem->FindTopolObject (0,TETRAHEDRON);
	// Node * ns[] = { fem->GetNode (3), fem->GetNode (4) };
	Node * ns[] = { tet->GetNode(0), tet->GetNode(1) };

	Simplex ts(EDGE, ns, 0);
	TryCollapseEdge (fem, ts, tet , 0, Centroid (ts,false) );
}

int state =1; 
void TestOneCollapse (FiniteElementModel*fem)
{
	if (!state)
	{
		TryCollapseLargeDihTetra (fem,
			(Tetrahedron*)
			fem->FindTopolObject (0, TETRAHEDRON),
			0.8);
		state ++;
	}
	else
	{
		LinkArray<Tetrahedron> tets=fem->TetrahedronArray ();
		fem->RemoveTetrahedra (&tets);
	}
}
